/* ----------------------------------------------------------------------
*
* Copyright (C) 2024 XiaoFei. All rights reserved.
*
* @file:        XiaoFei_ComplexMath.c
* @author:      XiaoFei
* @Date:        21-October-2024
* @Revision:    V1
* @Project:     XiaoFei DSP Library
*
* -------------------------------------------------------------------- */

#include "XiaoFei_ComplexMath.h"

/* 
* @function: 	My_FastSqrt
* @parameter:	[in] x					 	A pointer to the argument of a Square root.
* @return:		The Square root of x.
*/
static double My_FastSqrt(double x)
{
	double mi;
	mi = 1;
	if(x==0) return 0;
	if(x<0.5){
		while(x<0.1) 
        {
            x*=10;  
            mi*=0.316228; 
        }
		while(x<0.5) 
        { 
            x*=2;   
            mi*=0.707107; 
        }
	}
	if(x>2){
		while(x>10)  
        { 
            x/=10;  
            mi*=3.162278; 
        }
		while(x>2)   
        { 
            x/=2;   
            mi*=1.41421;  
        }
	}
	return mi*(0.07246-x*(x*(x*(x*(x*(x*(x*(0.1659*x-1.5238)+5.9278)-12.7419)+16.5698)-13.4677)+6.9649)-2.8225));
}

/* 
* @function: 	My_FastArctan
* @parameter:	[in] x					 	A pointer to the argument of a Arctangent function.
* @return:		The arctangent value of x.
*/
static double My_FastArctan(double x)
{
	if(x<-1)			
        return -(My_PI/2-(-(-1/x)*((-1/x)*((-1/x)*((-1/x)*((-1/x)*((-1/x)*((-1/x)*(0.004579*(-1/x)-0.03567)+0.09829)-0.0676)-0.2083)+0.4508)-0.03052)-0.997)));
	else if(x<0)	
        return -(-(-x)*((-x)*((-x)*((-x)*((-x)*((-x)*((-x)*(0.004579*(-x)-0.03567)+0.09829)-0.0676)-0.2083)+0.4508)-0.03052)-0.997));
	else if(x<=1)	
        return -x*(x*(x*(x*(x*(x*(x*(0.004579*x-0.03567)+0.09829)-0.0676)-0.2083)+0.4508)-0.03052)-0.997);
	else 					
        return My_PI/2-(-(1/x)*((1/x)*((1/x)*((1/x)*((1/x)*((1/x)*((1/x)*(0.004579*(1/x)-0.03567)+0.09829)-0.0676)-0.2083)+0.4508)-0.03052)-0.997));
}

/* 
* @function: 	My_FastExp
* @parameter:	[in] x					 	A pointer to the argument of a Exponential functions.
* @return:		The Exponential value of x.
*/
static double My_FastExp(double x)
{
	double mi;
	mi = 1;
	if((x>20)||(x<-20)) return 0;
	if(x>2)
    {
		while(x>5)	
        { 
            x-=5;		
            mi*=My_EXP_5;	
        }
		while(x>2)	
        {	
            x-=2;		
            mi*=My_EXP_2;	
        }		
	}
	if(x<-2)
    {
		while(x<-5)	
        {	
            x+=5;		
            mi/=My_EXP_5;	
        }
		while(x<-2)	
        {	
            x+=2;		
            mi/=My_EXP_2;	
        }		
	}
	return (1+x*(1+x*(0.5+x*(0.1667+x*(0.0417+x*(0.0083+x*(0.0014+x*(0.00022009+x*0.000026952))))))))*mi;
}

/* 
* @function: 	My_FastCos
* @parameter:	[in] x					 	A pointer to the argument of a Cosine function.
* @return:		The Cosine value of x.
*/
static double My_FastCos(double x)
{
	double flag;
	flag = 1;
	if((x>My_PI)||(x<-My_PI))
    {
		x -= ((int)(x/(2*My_PI)))*2*My_PI;
		if(x>My_PI)	
			x -= 2*My_PI;
		if(x<-My_PI)	
			x += 2*My_PI;
	}
	if((x<-My_PI/2)&&(x>-My_PI))
    {
		x = My_PI+x;
		flag = -1;
	}
	if((x>My_PI/2)&&(x<My_PI))
    {
		x = x-My_PI;
		flag = -1;
	}
	return (1-x*x*(0.4999-x*x*(0.0415-x*x*(0.0013-x*x*0.000017721))))*flag;
}

/* 
* @function: 	My_FastSin
* @parameter:	[in] x					 	A pointer to the argument of a Sinusoidal function.
*	@return:		The Sinusoidal value of x.
*/
static double My_FastSin(double x)
{
	if((x>My_PI)||(x<-My_PI))
    {
		x -= ((int)(x/(2*My_PI)))*2*My_PI;
		if(x>My_PI)	
			x -= 2*My_PI;
		if(x<-My_PI)	
			x += 2*My_PI;
	}
	if((x<-My_PI/2)&&(x>-My_PI))
		x = -My_PI-x;   
	if((x>My_PI/2)&&(x<My_PI))
		x = My_PI-x;		
	return x*(0.9988-x*x*(0.1649-x*x*(0.0077-x*x*0.00012975)));
}

/* 
* @function: 	My_FastLog
* @parameter:	[in] x					 	A pointer to the argument of a Logarithmic function.
*	@return:		The Logarithmic value of x.
*/
static double My_FastLog(double x)
{
	double mi;
	mi = 0;
	if(x<=0)	return 0;
	if(x<0.5)
    {
		while(x<0.1)	
        {	
            x*=10;	
            mi-=My_LOG_10;	
        }
		while(x<0.5)	
        {	
            x*=2;		
            mi-=My_LOG_2;		
        }
	}
	if(x>2)
    {
		while(x>10)
        {		
            x/=10;	
            mi+=My_LOG_10;	
        }
		while(x>2)
        {			
            x/=2;		
            mi+=My_LOG_2;		
        }
	}							
	return mi+0+1/(1/(x-1)+1/(2.0+1/(3/(x-1)+1/(1.0+1/(5/(x-1)+1/(2.0/3+1/(7/(x-1))))))));
}

/* 
* @function: 	My_ComplexAdd
* @parameter:	[in] 	*a					An addition to complex additions.
* @parameter:	[in] 	*b					Another addition to complex additions.
* @parameter:	[out] *ans				    The result of complex additions.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexAdd(Complex_TypeDef* a, Complex_TypeDef* b, Complex_TypeDef* ans)
{
	ans->Real = a->Real+b->Real;
	ans->Imag = a->Imag+b->Imag;
	return Success;
}

/* 
* @function: 	My_ComplexSub
* @parameter:	[in] 	*a					The subtracted number of complex subtraction.
* @parameter:	[in] 	*b					The subtraction of complex subtraction.
* @parameter:	[out] *ans				    The result of complex subtraction.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexSub(Complex_TypeDef* a, Complex_TypeDef* b, Complex_TypeDef* ans)
{
	ans->Real = a->Real-b->Real;
	ans->Imag = a->Imag-b->Imag;
	return Success;
}

/* 
* @function: 	My_ComplexMul
* @parameter:	[in] 	*a					A multiplier for complex multiplication.
* @parameter:	[in] 	*b					Another multiplier for complex multiplication.
* @parameter:	[out] *ans				    The result of complex multiplication.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexMul(Complex_TypeDef* a, Complex_TypeDef* b, Complex_TypeDef* ans)
{
	ans->Real = a->Real*b->Real-a->Imag*b->Imag;
	ans->Imag = a->Real*b->Imag+a->Imag*b->Real;
	return Success;
}

/* 
* @function: 	My_ComplexDiv
* @parameter:	[in] 	*a					The dividend number of complex division.
* @parameter:	[in] 	*b					The divisor of complex division.
* @parameter:	[out] *ans				    The result of complex division.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexDiv(Complex_TypeDef* a, Complex_TypeDef* b, Complex_TypeDef* ans)
{
	ans->Real = (a->Real*b->Real+a->Imag*b->Imag)/(b->Real*b->Real+b->Imag*b->Imag);
	ans->Imag = (a->Imag*b->Real-a->Real*b->Imag)/(b->Real*b->Real+b->Imag*b->Imag);
	return Success;
}

/* 
* @function: 	My_ComplexConj
* @parameter:	[in] 	*a					The original complex number of conjugate.
* @parameter:	[out] *ans				    The result of conjugation of complex numbers.
* @return:		Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexConj(Complex_TypeDef* a, Complex_TypeDef* ans)
{
	ans->Real = a->Real;
	ans->Imag = -a->Imag;
	return Success;
}

/* 
* @function:    My_ComplexMagn
* @parameter:   [in]    *a              The original complex number of Magnitude.
* @parameter:   [out]   *ans            The Magnitude of the complex numbers.
* @return:      Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexMagn(Complex_TypeDef* a, double* ans)
{
	*ans = My_FastSqrt(a->Real*a->Real+a->Imag*a->Imag);
	return Success;
}

/* 
* @function:    My_ComplexPhas
* @parameter:   [in]    *a              The original complex number of principal.
* @parameter:   [out]   *ans            The principal phase of the complex numbers.
* @return:      Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexPhas(Complex_TypeDef* a, double* ans)
{
	double test;
	if(a->Imag==0&&a->Real==0) 
		return Error;
	if(a->Imag==0&&a->Real>0)
    {
		*ans=0;
		return Success;		
	}
	if(a->Imag==0&&a->Real<0)
    {
		*ans=My_PI;
		return Success;		
	}	
	if(a->Imag>0&&a->Real==0)
    {
		*ans=My_PI/2;
		return Success;		
	}	
	if((a->Imag < 0) && (a->Real == 0))
    {
		*ans = -My_PI / 2;
		return Success;		
	}
	test = My_FastArctan(a->Imag / a->Real);
	if(a->Real > 0)
    {
		*ans = test;
		return Success;		
	}
	else if(a->Imag > 0)
    {
		*ans = test + My_PI;
		return Success;
	}
	else
    {
		*ans = test - My_PI;
		return Success;		
	}
}

/* 
* @function:    My_ComplexExp
* @parameter:   [in]    *a              The original complex number of Exponential.
* @parameter:   [out]   *ans            The Exponential of the complex numbers.
* @return:      Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexExp(Complex_TypeDef* a, Complex_TypeDef* ans)
{
	if((a->Real > 20) || (a->Real < -20))
		return Error;
	ans->Real = My_FastExp(a->Real) * My_FastCos(a->Imag);
	ans->Imag = My_FastExp(a->Real) * My_FastSin(a->Imag);
	return Success;
}

/* 
* @function:    My_ComplexLog
* @parameter:   [in]    *a              The original complex number of Logarithmic.
* @parameter:   [out]   *ans            The Logarithmic of the complex numbers.
* @return:      Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexLog(Complex_TypeDef* a, Complex_TypeDef* ans)
{
	ans->Real = My_FastLog(My_FastSqrt(a->Real * a->Real  + a->Imag * a->Imag));
	if(My_ComplexPhas(a,&ans->Imag) == Error)
		return Error;
	return Success;
}

/* 
* @function:    My_ComplexCos
* @parameter:   [in]    *a              The original complex number of Cosine.
* @parameter:   [out]   *ans            The Cosine of the complex numbers.
* @return:      Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexCos(Complex_TypeDef* a, Complex_TypeDef* ans)
{
	if((a->Imag > 20) || (a->Imag < -20))
		return Error;
	ans->Real = (1 / My_FastExp(a->Imag) + My_FastExp(a->Imag)) * My_FastCos(a->Real) / 2;
	ans->Imag = (1 / My_FastExp(a->Imag) - My_FastExp(a->Imag)) * My_FastSin(a->Real) / 2;
	return Success;
}

/* 
* @function:    My_ComplexSin
* @parameter:   [in]    *a              The original complex number of Sinusoidal.
* @parameter:   [out]   *ans            The Sinusoidal of the complex numbers.
* @return:      Successes or failures of the results of the calculations.
*/
ResStatus_TypeDef My_ComplexSin(Complex_TypeDef* a, Complex_TypeDef* ans)
{
	if((a->Imag > 20) || (a->Imag < -20))
		return Error;
	ans->Real = (My_FastExp(a->Imag) + 1 / My_FastExp(a->Imag)) * My_FastSin(a->Real) / 2;
	ans->Imag = (My_FastExp(a->Imag) - 1 / My_FastExp(a->Imag)) * My_FastCos(a->Real) / 2;
	return Success;
}




